//
// Created by ubuntu on 2024/1/4.
//

#include "subscriber_scan.h"

namespace line_extraction
{

SubcriberScan::SubcriberScan(
        ros::NodeHandle &nh, ros::NodeHandle &nh_local, std::string topic_name, size_t buff_size, std::string frame_ID) :
        nh_(nh), nh_local_(nh_local), frame_id_(frame_ID), data_cached_(false)
{
    loadParameters();
    subscriber_ = nh_.subscribe(topic_name, buff_size, &SubcriberScan::msg_callback, this);
    marker_publisher_ = nh_.advertise<visualization_msgs::Marker>(topic_name + "_markers", 1);
    if (is_split_and_merge) split_and_merge_.params_ = params_;
    else seed_area_growth_.params_ = params_;
    if (is_calc_error)
    {
        tf2_ros::Buffer tfBuffer;
        tf2_ros::TransformListener tfListener(tfBuffer);
        ros::Duration timeout(10.0); // 等待机器人描述的最长时间（以秒为单位）
        ros::Duration sleep_duration(0.2); // 休眠间隔
        ros::Time start_time = ros::Time::now();
        while (ros::ok() && (ros::Time::now() - start_time) < timeout)
        {
            try
            {
                transform = tfBuffer.lookupTransform("base_footprint", frame_id_, ros::Time(0));
                break; // 成功获取变换信息，退出循环
            }
            catch (tf2::TransformException &ex)
            {
                // ROS_WARN("Waiting for robot description: %s", ex.what());
            }
            ros::Duration(sleep_duration).sleep(); // 休眠一段时间再重试
        }
        tf2::Quaternion tf_quaternion;
        tf2::fromMsg(transform.transform.rotation, tf_quaternion);
        tf2::Matrix3x3(tf_quaternion).getRPY(roll, pitch, yaw);
        ROS_INFO_STREAM("get transform from base_footprint to " << std::fixed << std::setprecision(3)
                      << "\tpose: " << transform.transform.translation.x << " " << transform.transform.translation.y
                      << " " << transform.transform.translation.z << " rpy: " << roll << " " << pitch << " " << yaw);
    }
}

SubcriberScan::~SubcriberScan()
{
    subscriber_.shutdown();
    marker_publisher_.shutdown();
}

void SubcriberScan::loadParameters()
{
    ROS_DEBUG("*************************************");
    ROS_DEBUG("PARAMETERS: ");
    double bearing_std_dev, range_std_dev;
    nh_local_.param<double>("bearing_std_dev", bearing_std_dev, 1e-3);
    nh_local_.param<double>("range_std_dev", range_std_dev, 0.02);
    nh_local_.param<double>("least_sq_angle_thresh", params_.least_sq_angle_thresh, 1e-4);
    nh_local_.param<double>("least_sq_radius_thresh", params_.least_sq_radius_thresh, 1e-4);
    nh_local_.param<double>("max_line_gap", params_.max_line_gap, 0.2);
    nh_local_.param<double>("min_line_length", params_.min_line_length, 0.1);
    nh_local_.param<double>("min_range", params_.min_range, 0.1);
    nh_local_.param<double>("max_range", params_.max_range, 40.5);
    nh_local_.param<double>("min_split_dist", params_.min_split_dist, 0.01);
    nh_local_.param<double>("outlier_dist", params_.outlier_dist, 0.01);  // 5cm
    nh_local_.param<int>("min_line_points", params_.min_line_points, 5);
    params_.bearing_var = bearing_std_dev * bearing_std_dev;
    params_.range_var = range_std_dev * range_std_dev;
    ROS_DEBUG("bearing_var: %f", params_.bearing_var);
    ROS_DEBUG("range_var: %f", params_.range_var);
    ROS_DEBUG("least_sq_angle_thresh: %f", params_.least_sq_angle_thresh);
    ROS_DEBUG("least_sq_radius_thresh: %f", params_.least_sq_radius_thresh);
    ROS_DEBUG("max_line_gap: %f", params_.max_line_gap);
    ROS_DEBUG("min_line_length: %f", params_.min_line_length);
    ROS_DEBUG("min_range: %f", params_.min_range);
    ROS_DEBUG("max_range: %f", params_.max_range);
    ROS_DEBUG("min_split_dist: %f", params_.min_split_dist);
    ROS_DEBUG("outlier_dist: %f", params_.outlier_dist);
    ROS_DEBUG("min_line_points: %d", params_.min_line_points);
    ROS_DEBUG("*************************************");
}

void SubcriberScan::msg_callback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
    buff_mutex_.lock();
    if (!data_cached_)
    {
        cacheData(scan_msg);
        data_cached_ = true;
    }
    // 激光雷达测量数据
    std::vector<double> ranges(scan_msg->ranges.begin(), scan_msg->ranges.end());
    Rangedata range_data_;   // 数据流
    range_data_.ranges = ranges;
    range_data_.xs.clear();
    range_data_.ys.clear();
    for (unsigned int indice : cs_data_.index)
    {
        range_data_.xs.push_back(cs_data_.cos_value[indice] * ranges[indice]);
        range_data_.ys.push_back(cs_data_.sin_value[indice] * ranges[indice]);
    }
    new_scan_data_.push_back(range_data_);
    buff_mutex_.unlock();
}

void SubcriberScan::startRun()
{
    // pipe all available measurements to output buffer:
    while (!new_scan_data_.empty())
    {
        std::vector<gline> glines;
        // Extract the lines
        if (is_split_and_merge)
        {
            std::vector<Line> lines;
            split_and_merge_.range_data_ = new_scan_data_.front();
            split_and_merge_.extractLines(lines);
            for (auto &item : lines)
            {
                gline gl;
                gl.x1 = item.getStart()[0];
                gl.y1 = item.getStart()[1];
                gl.x2 = item.getEnd()[0];
                gl.y2 = item.getEnd()[1];
                glines.push_back(gl);
            }
        }
        else
        {
            std::vector<line> lines;
            seed_area_growth_.range_data_ = new_scan_data_.front();
            seed_area_growth_.extractLines(lines, glines);
        }
        populateMarkerMsg(glines);
        // Also publish markers if parameter publish_markers is set to true
        new_scan_data_.pop_front();
    }
}

void SubcriberScan::cacheData(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
    const std::size_t num_measurements = std::ceil(
            (scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment);
    for (std::size_t i = 0; i < num_measurements; ++i)
    {
        const double b = scan_msg->angle_min + i * scan_msg->angle_increment;
        if (is_split_and_merge)
        {
            split_and_merge_.cs_data_.bearings.push_back(b);
            split_and_merge_.cs_data_.cos_value.push_back(cos(b));
            split_and_merge_.cs_data_.sin_value.push_back(sin(b));
            split_and_merge_.cs_data_.index.push_back(i);
        }
        else
        {
            seed_area_growth_.cs_data_.bearings.push_back(b);
            seed_area_growth_.cs_data_.cos_value.push_back(cos(b));
            seed_area_growth_.cs_data_.sin_value.push_back(sin(b));
            seed_area_growth_.cs_data_.index.push_back(i);
        }
    }
    ROS_DEBUG("Data has been cached.");
}

void SubcriberScan::populateMarkerMsg(const std::vector<gline> &lines)
{
    visualization_msgs::Marker marker_msg;
    marker_msg.ns = "line_extraction";
    marker_msg.id = 0;
    marker_msg.type = visualization_msgs::Marker::LINE_LIST;
    marker_msg.scale.x = 0.01;
    marker_msg.color.r = 1.0;
    marker_msg.color.g = 0.0;
    marker_msg.color.b = 0.0;
    marker_msg.color.a = 0.5;
    geometry_msgs::Point p_start;
    geometry_msgs::Point p_end;
    ROS_INFO_STREAM("lines: " << lines.size());
    for (const auto &line : lines)
    {
        p_start.x = line.x1;
        p_start.y = line.y1;
        p_end.x = line.x2;
        p_end.y = line.y2;
        marker_msg.points.push_back(p_start);
        marker_msg.points.push_back(p_end);
    }
    if (is_calc_error)
    {
        double d1 = sqrt(pow(p_start.x, 2) + pow(p_start.y, 2)) * sin(pitch) - transform.transform.translation.z;
        double d2 = sqrt(pow(p_end.x, 2) + pow(p_end.y, 2)) * sin(pitch) - transform.transform.translation.z;
        ROS_INFO_STREAM("p_start: " << p_start.x << " " << p_start.y << " p_end: " << p_end.x << " " << p_end.y
                                    << " d1: " << d1 << " d2: " << d2);
    }
    marker_msg.header.frame_id = frame_id_;
    marker_msg.header.stamp = ros::Time::now();
    marker_publisher_.publish(marker_msg);
}

}
